package com.tumblr.logico2012.gestures;

import java.util.HashMap;
import java.util.Vector;

import processing.core.PApplet;
import processing.core.PImage;
import processing.core.PVector;
import SimpleOpenNI.IntVector;
import SimpleOpenNI.SimpleOpenNI;

import com.tumblr.logico2012.IApplet;
import static com.tumblr.logico2012.AppletLogico.DISABLE_CAMERA;

/**
 * The gesture manager has two purposes: - it monitors parts of the skeletons of
 * the users and make some computation. - it maintains a list of gestures that
 * are currently active and triggers their events.
 * 
 * The gesture implementation can access the information collected by the
 * manager to try and identify gestures.
 * 
 * The manager has two kinds of "listeners" or "monitors": - Segment: a segment
 * defined by two points of the skeleton of a user. Following information are
 * provided: A vector defining this segment is projected on the planes XY, XZ
 * and YZ. For each projection, the angle between the projected vector and the
 * reference vector is calculated. - Angle: the angle between two segment of the
 * skeleton of a user.
 * 
 * For both monitors, every FRAMESTEP frames, the current values are compared
 * with the previous values and the delta is stored. This provides a rudimentary
 * speed measurement and helps describe the dynamic of a movement.
 * 
 * Gesture implementations can trigger a refractory period for the monitors they
 * have used when a gesture has been identify to prevent a gesture being
 * recognized when the user takes back his/her hand.
 */
public class GestureManager {
	public final static int FRAMESTEP = 5;
	public final static float REFRACTORY_PERIOD = 0.5f; // seconds

	public final static int GESTUREBAR_WIDTH = 100;
	public final static int FACE_SIZE = 200;
	public final static int ICON_SIZE = 60;
	
	/**
	 * "Angle-Monitor": stores information about an angle between two skeleton
	 * segment.
	 */
	public class Angle {
		int m_userId; // user
		int m_joint1; // first jointure
		int m_joint2; // second jointure
		int m_joint3; // third jointure
		int m_refractory; // counter for refractory time

		public float m_angle; // current angle
		public float m_prev_angle; // previous angle (block of FRAMESTEP frames)
		public float m_delta_angle; // current - previous
		public boolean m_hasPrevious, m_hasDelta; // current state of
													// information collect

		Angle(int userId, int joint1, int joint2, int joint3) {
			m_userId = userId;
			m_joint1 = joint1;
			m_joint2 = joint2;
			m_joint3 = joint3;
			m_hasPrevious = false;
			m_hasDelta = false;
		}

		void setRefractory() {
			m_refractory = (int) (p.frameRate * REFRACTORY_PERIOD);
		}

		void setPrevious(float angle) {
			m_prev_angle = angle;
			if (!m_hasPrevious) {
				m_hasPrevious = true;
			}
		}

		void update(float angle, boolean updateDelta) {
			// update is called for each frame but previous and delta are not
			// updated every time
			if (m_refractory > 0)
				m_refractory--;
			m_angle = angle;
			if (updateDelta) {
				if (m_hasPrevious) {
					m_delta_angle = angle - m_prev_angle;
					m_hasDelta = true;
				}
				setPrevious(angle);
			}
		}
	}

	/**
	 * "Segment-Monitor": stores information about a skeleton segment.
	 */
	public class Segment {
		int m_userId; // user
		int m_joint1; // first jointure
		int m_joint2; // second jointure
		int m_refractory; // counter for refractory period

		public PVector m_xy; // Projection vector on XY, "front view" (2D
								// vector)
		public PVector m_xz; // Projection vector on XZ, "top view" (2D vector)
		public PVector m_yz; // Projection vector on YZ, "side view" (2D vector)
		public PVector m_angles; // Angles between the projection vectors and a
									// reference vector:
		// x: XY, y: XZ, z: YZ - Note: this is not actually a vector, but
		// PVector is convenient.
		public PVector m_prev_xy, m_prev_xz, m_prev_yz, m_prev_angles; // previous
																		// values
		public PVector m_delta_xy, m_delta_xz, m_delta_yz, m_delta_angles; // current
																			// -
																			// previous
																			// values
		public boolean m_hasPrevious, m_hasDelta; // state of the information
													// collect

		Segment(int userId, int joint1, int joint2) {
			m_userId = userId;
			m_joint1 = joint1;
			m_joint2 = joint2;
			m_hasPrevious = false;
			m_hasDelta = false;
		}

		void setRefractory() {
			m_refractory = (int) (p.frameRate * REFRACTORY_PERIOD);
		}

		void setPrevious(PVector xy, PVector xz, PVector yz, PVector angles) {
			m_prev_xy = new PVector(xy.x, xy.y, 0);
			m_prev_xz = new PVector(xz.x, xz.y, 0);
			m_prev_yz = new PVector(yz.x, yz.y, 0);
			m_prev_angles = new PVector(angles.x, angles.y, angles.z);
			if (!m_hasPrevious) {
				m_hasPrevious = true;
			}
		}

		void update(PVector xy, PVector xz, PVector yz, PVector angles,
				boolean updateDelta) {
			// update is called for each frame but previous and delta are not
			// updated every time
			if (m_refractory > 0)
				m_refractory--;
			m_xy = new PVector(xy.x, xy.y, 0);
			m_xz = new PVector(xz.x, xz.y, 0);
			m_yz = new PVector(yz.x, yz.y, 0);
			m_angles = new PVector(angles.x, angles.y, angles.z);
			if (updateDelta) {
				if (m_hasPrevious) {
					m_delta_xy = new PVector(xy.x - m_prev_xy.x, xy.y
							- m_prev_xy.y, 0);
					m_delta_xz = new PVector(xz.x - m_prev_xz.x, xz.y
							- m_prev_xz.y, 0);
					m_delta_yz = new PVector(yz.x - m_prev_yz.x, yz.y
							- m_prev_yz.y, 0);
					m_delta_angles = new PVector(angles.x - m_prev_angles.x,
							angles.y - m_prev_angles.y, angles.z
									- m_prev_angles.z);
					m_hasDelta = true;
				}
				setPrevious(xy, xz, yz, angles);
			}
		}
	}

	private HashMap<String,PImage> m_icons;
	private HashMap<String,String> m_gestureInstructions;
	private Vector<Segment> m_segments; // list of segments to monitor
	private Vector<Angle> m_angles; // list of angles to monitor
	private Vector<IGesture> m_gestures;// list of active gestures
	private int m_framecounter; // frame counter
	public SimpleOpenNI m_context; // SimpleOpenNI
	public IApplet m_app; // Main class
	public PApplet p; // Processing engine

	public GestureManager(IApplet app, PApplet processor, SimpleOpenNI context) {
		m_context = context;
		p = processor;
		m_app = app;
		reset();
		
		m_icons = new HashMap<String,PImage>();
		addIcon("swipeRight", "swipeRight.jpg");
		addIcon("swipeLeft", "swipeLeft.jpg");
		addIcon("swipeRightOut", "swipeRightOut.jpg");
		addIcon("swipeLeftOut", "swipeLeftOut.jpg");
		addIcon("click", "click.jpg");
		addIcon("noicon", "testicon.jpeg");
		
		m_gestureInstructions = new HashMap<String,String>();
	}

	/**
	 * Associate a text (instruction) with a gesture icon
	 * @param gestureName Gesture name (actually the name of the gesture icon used to display the gesture)
	 * @param instruction Instruction
	 */
	public void setGestureInstruction(String gestureName, String instruction) {
		m_gestureInstructions.put(gestureName, instruction);
	}
	
	/**
	 * Perform processing transformation to set a drawing area between the side bars:
	 * After a call of setDrawingArea, (0,0) corresponds to the corner of the drawing area
	 * instead of the corner of the screen.
	 * The transformation applies for the current frame only.
	 */
	public void setDrawingArea() {
		p.translate(GESTUREBAR_WIDTH, 0);
		p.scale((float) (p.width - 2 * GESTUREBAR_WIDTH) / p.width, 1);
	}

	/**
	 * Register a gesture icon.
	 * @param name Gesture name
	 * @param file Icon file
	 */
	public void addIcon(String name, String file) {
		m_icons.put(name, p.loadImage(file));
	}

	/**
	 * Extract a picture of the head of a given user.
	 * @param userId User
	 * @param camera Camera picture (camera.loadPixels must have been called)
	 * @return Picture or null
	 */
	public PImage getFace(int userId, PImage camera) {
		PVector posHead3D = new PVector(), posHead2D = new PVector();
		if (m_context.isTrackingSkeleton(userId)) {
			if ((m_context.getJointPositionSkeleton(userId,
					SimpleOpenNI.SKEL_HEAD, posHead3D)) < 0.001f) {
				return null;
			}
		} else
			return null;

		m_context.convertRealWorldToProjective(posHead3D, posHead2D);
		
		PImage face = new PImage(FACE_SIZE, FACE_SIZE);

		face.loadPixels();
		int x, y, xx, yy;

		for (y = 0, yy = (int)posHead2D.y-FACE_SIZE/2; y < FACE_SIZE; y++, yy++) {
			for (x = 0, xx = (int)posHead2D.x-FACE_SIZE/2; x < FACE_SIZE; x++, xx++) {
				face.pixels[y * FACE_SIZE + x] = (xx >= 0 && xx < camera.width && yy >= 0 && yy < camera.height) 
				? camera.pixels[yy*camera.width + xx] : p.color(255,255,255);
			}
		}
		face.updatePixels();

		return face;
	}

	/**
	 * Draw the gesture bars
	 */
	public void drawGestureBars() {
		int y, x, userId;
		IntVector users = new IntVector();
		m_context.getUsers(users);
		
		PImage camera;
		if(!DISABLE_CAMERA) {
			camera = m_context.depthImage();
			camera.loadPixels();
		}
		
		Vector<String> iconNames;
		boolean[] iconStates;
		
		for (int i = 0; i < 2; i++) {
			p.pushMatrix();
			p.translate(i * (p.width - GESTUREBAR_WIDTH), 0);
			p.fill(255,255,255);
			p.stroke(0,0,0);
			p.strokeWeight(3);
			p.rect(0, 0, GESTUREBAR_WIDTH, p.height);

			if (users.size() > i) {
				userId = users.get(i);

				// get the list of gesture icons
				iconNames = new Vector<String>();
				iconStates = new boolean[m_gestures.size()];
				for (IGesture gesture : m_gestures) {
					if(gesture.getUserId() == userId) {
						int idx;
						if((idx = iconNames.indexOf(gesture.getIcon())) >= 0) {
							// this gesture uses an icon that is already in the list, we just need to eventually
							// update its active state
							if(gesture.isActive() && gesture.showBorderOnSelect()) {
								iconStates[idx] = true;
							}
						} else {
							iconNames.add(gesture.getIcon());
							iconStates[iconNames.size() - 1] = gesture.isActive() && gesture.showBorderOnSelect();
						}
					}
				}
				
				// display the bar
				y = 20;
				for(int k = 0; k < iconNames.size(); k++) {
					PImage icon = m_icons.get(iconNames.get(k));
					p.image(icon, x = (GESTUREBAR_WIDTH - ICON_SIZE) / 2, y, ICON_SIZE, ICON_SIZE);
					if (iconStates[k]) {
						p.noFill();
						p.stroke(255, 0, 0);
						p.strokeWeight(3);
						p.rect(x - 10, y - 10, ICON_SIZE + 20, ICON_SIZE + 20);
					}
					y += ICON_SIZE + 20;

					String instr = m_gestureInstructions.get(iconNames.get(k));
					if(instr != null) {
						if(!instr.equals("")) {
							p.textFont(p.createFont("Georgia", 12));
							p.fill(0,0,0);
							p.textAlign(PApplet.TOP, PApplet.LEFT);
							p.text(instr, (GESTUREBAR_WIDTH - p.textWidth(instr)) / 2, y + 2);
							y += 20 + 2*(12+2);
						}
					}
				}
				
				PImage face;
				if(DISABLE_CAMERA) face = null;
				else			   face = getFace(userId, camera);
				if (face != null)
					p.image(face, 0, p.height - GESTUREBAR_WIDTH, GESTUREBAR_WIDTH - 3, GESTUREBAR_WIDTH - 3);
			}
			p.popMatrix();
		}
	}

	/**
	 * Get a segment
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 * @return Segment if monitored or null
	 */
	public Segment getSegment(int userId, int joint1, int joint2) {
		for (Segment segment : m_segments) {
			if (segment.m_userId == userId && segment.m_joint1 == joint1
					&& segment.m_joint2 == joint2) {
				if (segment.m_refractory > 0)
					return null;
				else
					return segment;
			}
		}
		return null;
	}

	/**
	 * Start the monitoring of a segment
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 */
	public void enableSegment(int userId, int joint1, int joint2) {
		if (getSegment(userId, joint1, joint2) == null)
			m_segments.add(new Segment(userId, joint1, joint2));
	}

	/**
	 * Get an angle
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 * @param joint3 Third jointure
	 * @return Angle if monitored, or null
	 */
	public Angle getAngle(int userId, int joint1, int joint2, int joint3) {
		for (Angle angle : m_angles) {
			if (angle.m_userId == userId && angle.m_joint1 == joint1
					&& angle.m_joint2 == joint2 && angle.m_joint3 == joint3) {
				if (angle.m_refractory > 0)
					return null;
				else
					return angle;
			}
		}
		return null;
	}

	/**
	 * Start the monitoring of an angle.
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 * @param joint3 Third jointure
	 */
	public void enableAngle(int userId, int joint1, int joint2, int joint3) {
		if (getAngle(userId, joint1, joint2, joint3) == null)
			m_angles.add(new Angle(userId, joint1, joint2, joint3));
	}

	/**
	 * Reset the manager: - Stop all monitors, remove gestures and users -
	 * Register all current users for the current screen
	 * 
	 * This must be done when changing screens: gestures and monitors activated
	 * by the previous screen are removed, and the users are registered on the
	 * new screen.
	 */
	public void reset() {
		m_segments = new Vector<Segment>();
		m_angles = new Vector<Angle>();
		m_gestures = new Vector<IGesture>();
		m_framecounter = 0;

		if(!DISABLE_CAMERA) {
			IntVector aUsers = new IntVector();
			m_context.getUsers(aUsers);
			for (int i = 0; i < aUsers.size(); i++) {
				int userId = aUsers.get(i);
				if (m_app.getCurrentScreen() != null)
					m_app.getCurrentScreen().registerUser(this, userId);
			}
		}
	}	

	/**
	 * Remove all monitors and gestures associated with a user. This is useful
	 * when a user is lost: his/her informations are not relevant anymore.
	 * 
	 * @param userId User
	 */
	public void unregisterUser(int userId) {
		for (int i = m_gestures.size() - 1; i >= 0; i--)
			if (m_gestures.get(i).getUserId() == userId)
				m_gestures.remove(i);
		for (int i = m_segments.size() - 1; i >= 0; i--)
			if (m_segments.get(i).m_userId == userId)
				m_segments.remove(i);
		for (int i = m_angles.size() - 1; i >= 0; i--)
			if (m_angles.get(i).m_userId == userId)
				m_angles.remove(i);
	}

	/**
	 * Add a gesture
	 * 
	 * @param gesture Gesture
	 */
	public void addGesture(IGesture gesture) {
		m_gestures.add(gesture);
		gesture.init(this);
	}

	/**
	 * Update the state of the manager
	 */
	public void update() {
		// update the segments
		PVector xy = new PVector(), xz = new PVector(), yz = new PVector(), angles = new PVector();
		for (Segment segment : m_segments) {
			if (m_context.isTrackingSkeleton(segment.m_userId)) {
				if (computeSegment(segment.m_userId, segment.m_joint1,
						segment.m_joint2, xy, xz, yz, angles) > 0.001f) {
					segment.update(xy, xz, yz, angles, !segment.m_hasPrevious
							|| m_framecounter % FRAMESTEP == 0);
				} else {
					segment.m_hasPrevious = false;
					segment.m_hasDelta = false;
				}
			} else {
				segment.m_hasPrevious = false;
				segment.m_hasDelta = false;
			}
		}

		// update the angles
		Angle _angle = new Angle(0, 0, 0, 0);
		for (Angle angle : m_angles) {
			if (m_context.isTrackingSkeleton(angle.m_userId)) {
				if (computeAngle(angle.m_userId, angle.m_joint1,
						angle.m_joint2, angle.m_joint3, _angle) > 0.001f) {
					angle.update(_angle.m_angle, !angle.m_hasPrevious
							|| m_framecounter % FRAMESTEP == 0);
				} else {
					angle.m_hasPrevious = false;
					angle.m_hasDelta = false;
				}
			} else {
				angle.m_hasPrevious = false;
				angle.m_hasDelta = false;
			}
		}

		// check for gesture events
		for (IGesture gesture : m_gestures) {
			if(!gesture.isInit()) gesture.init(this);
			if (gesture.checkForStartEvent(this))
				gesture.gestureStarted(this);
			if (gesture.checkForStopEvent(this)) {
				if (gesture.isAborted(this))
					gesture.gestureAborted(this);
				else
					gesture.gestureEnded(this);
				gesture.setRefractory(this);
			}
		}

		m_framecounter++;
	}

	/**
	 * Computations for a monitored angle
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 * @param joint3 Third jointure
	 * @param angle  Output parameter: Angle object - The result of the computation
	 *               is stored in angle.m_angle. The rest of the object is not
	 *               relevant.
	 * @return Confidence (if < 0.001f, no result)
	 */
	float computeAngle(int userId, int joint1, int joint2, int joint3,
			Angle angle) {
		float confidence;

		// get the position for the 3 jointures
		PVector pos1 = new PVector(), pos2 = new PVector(), pos3 = new PVector();
		if ((confidence = m_context.getJointPositionSkeleton(userId, joint1,
				pos1)) < 0.001f) {
			return confidence;
		}
		if ((confidence = m_context.getJointPositionSkeleton(userId, joint2,
				pos2)) < 0.001f) {
			return confidence;
		}
		if ((confidence = m_context.getJointPositionSkeleton(userId, joint3,
				pos3)) < 0.001f) {
			return confidence;
		}

		// compute the vectors describing both segments
		PVector vec1, vec2;
		vec1 = new PVector(pos1.x - pos2.x, pos1.y - pos2.y, pos1.z - pos2.z);
		vec2 = new PVector(pos3.x - pos2.x, pos3.y - pos2.y, pos3.z - pos2.z);

		// compute the angle
		angle.m_angle = new Float((float) Math.acos(((vec1.x * vec2.x)
				+ (vec1.y * vec2.y) + (vec1.z * vec2.z))
				/ (Math.sqrt(vec1.x * vec1.x + vec1.y * vec1.y + vec1.z
						* vec1.z) * Math.sqrt(vec2.x * vec2.x + vec2.y * vec2.y
						+ vec2.z * vec2.z))));

		return confidence;
	}

	/**
	 * Computations for a monitored segment
	 * 
	 * @param userId User
	 * @param joint1 First jointure
	 * @param joint2 Second jointure
	 * @param xy Output parameter: projection vector on XY
	 * @param xz Output parameter: projection vector on XZ
	 * @param yz Output parameter: projection vector on YZ
	 * @param angles Output parameter: angles
	 * @return
	 */
	float computeSegment(int userId, int joint1, int joint2, PVector xy,
			PVector xz, PVector yz, PVector angles) {
		float confidence;

		// Get the position of the jointures
		PVector pos1 = new PVector(), pos2 = new PVector();
		if ((confidence = m_context.getJointPositionSkeleton(userId, joint1,
				pos1)) < 0.001f) {
			return confidence;
		}
		if ((confidence = m_context.getJointPositionSkeleton(userId, joint2,
				pos2)) < 0.001f) {
			return confidence;
		}
		// compute the vector for the segment
		PVector pos = new PVector(pos1.x - pos2.x, pos1.y - pos2.y, pos1.z
				- pos2.z);

		// we "normalize" the vector so the length is between -1 and 1
		// with the exact value depending on the ratio ( size of projection
		// vector / size of segment )
		// For example:
		// We consider the segment elbow-hand.
		// The user is holding his/her arm horizontally orthogonally to his/her
		// body, the size
		// of the projection vector on YZ (side view) will be close to 1 (full
		// arm length).
		// If s/he holds his/her arm against his/her body, the same vector will
		// be close to 0, since the
		// arm is barely visible in this view.
		//
		// The normalization makes the data user-independant.
		float normalize = (float) (1 / Math.sqrt(pos.x * pos.x + pos.y * pos.y
				+ pos.z * pos.z));
		xy.x = pos.x * normalize;
		xy.y = pos.y * normalize;
		angles.x = (xy.y < 0 ? -1 : 1)
				* (float) Math
						.acos(xy.x / Math.sqrt(xy.x * xy.x + xy.y * xy.y));

		xz.x = pos.x * normalize;
		xz.y = pos.z * normalize;
		angles.y = (xz.y < 0 ? -1 : 1)
				* (float) Math
						.acos(xz.x / Math.sqrt(xz.x * xz.x + xz.y * xz.y));

		yz.x = pos.y * normalize;
		yz.y = pos.z * normalize;
		angles.z = (yz.y < 0 ? -1 : 1)
				* (float) Math
						.acos(yz.x / Math.sqrt(yz.x * yz.x + yz.y * yz.y));

		return confidence;
	}

}
